﻿using LightCAD.MathLib.CSG;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Security.Cryptography;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Linq;

namespace LightCAD.MathLib.CSG
{
   class GeomBlob<GeomIdx>
    {
        public BBox3 bbox;
        public Vec3 point; // representative point, usually the box midpoint
        public GeomIdx id;
    };

   class AABVHNode<GeomIdx>
    {
        public const int LEAF_SIZE = 8;

        public static int index;
        public int i;
        public BBox3 bbox = new BBox3();
        public AABVHNode<GeomIdx> left;
        public AABVHNode<GeomIdx> right;
        public List<int> blobids = new List<int>().makeSize(LEAF_SIZE);

        public AABVHNode()
        {
            i = index++;
        }
        public bool isLeaf() { return left == null; }

    };



   class AABVH<GeomIdx>
    {
        public const int LEAF_SIZE = 8;
        public AABVH(List<GeomBlob<GeomIdx>> geoms)
        {
            this.root = null;
            this.blobs.AddRange(geoms);
            tmpids.resize(geoms.size());
            Utils.ENSURE(blobs.size() > 0);

            for (int k = 0; k < tmpids.size(); k++)
                tmpids[k] = k;

            root = constructTree(0, tmpids.size(), 2);
        }

        // query?
        public void for_each_in_box(BBox3 bbox,int tref, Action<GeomIdx> action)
        {
            // do a recursive search and invoke the action at each
            // piece of geometry
            var nodes = new Stack<AABVHNode<GeomIdx>>();
            nodes.Push(root);

            while (nodes.Count > 0)
            {
                AABVHNode<GeomIdx> node = nodes.Peek();
                nodes.Pop();

                // check bounding box isct
                if (!BBox3.hasIsct(node.bbox, bbox)) continue;

                // otherwise...
                if (node.isLeaf())
                {
                    //Debug.Print("node:" + node.i);
                    
                    for (int ind = 0; ind != node.blobids.size(); ++ind)
                    {
                        
                        int bid = node.blobids[ind];
                        if (tref == 7540 && node.i == 3741 && bid== 6026)
                        {
                        }
                        if (BBox3.hasIsct(bbox, blobs[bid].bbox))
                        {
                            //Debug.Print("ind:" + bid);
                            action(blobs[bid].id);
                        }
                    }
                }
                else
                {
                    nodes.Push(node.left);
                    nodes.Push(node.right);
                }
            }
        }

        // process range of tmpids including begin, excluding end
        // last_dim provides a hint by saying which dimension a
        // split was last made along
        AABVHNode<GeomIdx> constructTree(int begin, int end, int last_dim)
        {
            Utils.ENSURE(end - begin > 0); // don't tell me to build a tree from nothing
                                           // base case
            if (end - begin <= LEAF_SIZE)
            {
                var tnode = node_pool.alloc().datum;
                tnode.left = null;
                tnode.blobids.resize(end - begin);
                for (int k = 0; k < end - begin; k++)
                {
                    int blobid = tnode.blobids[k] = tmpids[begin + k];
                    tnode.bbox = BBox3.convex(tnode.bbox, blobs[blobid].bbox);
                }
                return tnode;
            }
            // otherwise, let's try to split this geometry up

            int dim = (last_dim + 1) % 3;
            int mid = (begin + end) / 2;
            quickSelect(mid, begin, end, dim);

            // now recurse
            var node = node_pool.alloc().datum;
            node.left = constructTree(begin, mid, dim);
            node.right = constructTree(mid, end, dim);
            node.bbox = BBox3.convex(node.left.bbox, node.right.bbox);
            return node;
        }
        private int rndIdx = 1;
        // precondition: begin <= select < end
        void quickSelect(int select, int begin, int end, int dim)
        {
            // NOTE: values equal to the pivot
            //       may appear on either side of the split
            if (end - 1 == select) return;

            // p(ivot)i(ndex) and p(ivot)v(alue)
            // int pi =MathEx.RandomInt()%(end - begin) + begin;
            int pi = (rndIdx++) % (end - begin) + begin;
            double pv = blobs[tmpids[pi]].point[dim];

            int front = begin;
            int back = end - 1;
            while (front < back)
            {
                if (blobs[tmpids[front]].point[dim] < pv)
                {
                    front++;
                }
                else if (blobs[tmpids[back]].point[dim] > pv)
                {
                    back--;
                }
                else
                {
                    Utils.swap(tmpids, front, back);
                    front++;
                    back--;
                }
            }
            if (front == back && blobs[tmpids[front]].point[dim] <= pv)
            {
                front++;
            }

            if (select < front)
            {
                quickSelect(select, begin, front, dim);
            }
            else
            {
                quickSelect(select, front, end, dim);
            }
        }
        AABVHNode<GeomIdx> root;

        IterPool<AABVHNode<GeomIdx>> node_pool = new IterPool<AABVHNode<GeomIdx>>();
        List<GeomBlob<GeomIdx>> blobs = new List<GeomBlob<GeomIdx>>();
        List<int> tmpids = new List<int>(); // used during construction
    };
}
